# @Time : 2020/9/23 16:08
# @Author : Fioman 
# @Phone : 13149920693
import cv2 as cv
import numpy as np
import time
import os
import imutils


def image_show(name, image, time, show_state=False):
    if True:
        rate = 6
        width, height = int(image.shape[1] / rate), int(image.shape[0] / rate)
        showImage = cv.resize(image, (width, height), interpolation=cv.INTER_AREA)
        cv.namedWindow(name, cv.WINDOW_AUTOSIZE)
        cv.imshow(name, showImage)
        cv.waitKey(0)


def find_all_board(image, width, height, filterThres, areaRate):
    T, board = cv.threshold(image, filterThres + 15, 255, cv.THRESH_BINARY)
    print("TotalBoard 阈值 = {}".format(T))
    image_show("Board", board, 1, True)
    cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaRate * width * height]
    centerPoints = [(int(cv.minAreaRect(contour)[0][0]), int(cv.minAreaRect(contour)[0][1])) for contour in largeContours]
    for index, center in enumerate(centerPoints):
        anotherCenter = centerPoints[(index + 1) % len(centerPoints)]
        cv.line(board, center, anotherCenter, 255, 20)
    cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    cntFinalFinded = sorted(cnts, key=cv.contourArea, reverse=True)[0]
    box = cv.minAreaRect(cntFinalFinded)
    (cx, cy), (w, h), angle = box
    if angle > 70:
        angle = 90 - angle
        box = (cx, cy), (h, w), angle
    elif angle < -70:
        angle = 90 + angle
        box = (cx, cy), (h, w), angle

    rectPoints = np.int0(cv.boxPoints(box))
    boxMask = np.zeros(image.shape, dtype=np.uint8)
    cv.drawContours(boxMask, [rectPoints], -1, 255, -1)
    imageBlackAround = cv.bitwise_and(image, boxMask)
    return imageBlackAround, box


def get_width_and_height(fileName):
    """根据文件名或者宽度和高度"""
    if fileName[-4:] != ".bmp":
        return None, None
    if "w_" not in fileName or "h_" not in fileName:
        return None, None

    widthIndex = fileName.index("w_")
    heightIndex = fileName.index("h_")
    width = int(fileName[widthIndex + 2:widthIndex + 6])
    height = int(fileName[heightIndex + 2:heightIndex + 6])
    return width, height


def get_rect_points_by_clockwise(rectPoints):
    """将矩形的四个顶点按照顺时针方向排列"""
    # 先按照坐标和进行排列,坐标和最小的肯定是左上角,最大的肯定是右下角.
    # 另外两个点,按x坐标大小进行排列,x大的是右上角,x小的是左下角.
    rectPoints = sorted(rectPoints, key=np.sum)
    leftTop = rectPoints[0]
    rightBottom = rectPoints[3]

    if rectPoints[1][0] > rectPoints[2][0]:
        rightTop = rectPoints[1]
        leftBottom = rectPoints[2]
    else:
        rightTop = rectPoints[2]
        leftBottom = rectPoints[1]

    return leftTop, rightTop, rightBottom, leftBottom


def distance(p1, p2):
    """计算平面坐标系中两个点之间的距离"""
    assert len(p1) == 2
    assert len(p2) == 2
    dis = np.sqrt(np.square(p2[0] - p1[0]) + np.square(p2[1] - p1[1]))
    return round(dis, 2)


def check_box(cntsSorted, width, height, totalBoardColor, calcThres):
    """根据轮廓和阈值检测,我们找到的轮廓是否满足要求"""
    assert len(cntsSorted) >= 2
    res = {"result": "fail", "upBox": None, "downBox": None, "info": "", "calcSize": None, "calcErr": None}

    try:
        widthMax = width + calcThres * MM2PIX
        widthMin = width - calcThres * MM2PIX
        heightMax = height + calcThres * MM2PIX
        heightMin = height - calcThres * MM2PIX

        box1 = cv.minAreaRect(cntsSorted[0])
        box2 = cv.minAreaRect(cntsSorted[1])
        # 先确定是上轮廓还是下轮廓,根据中心点坐标.上轮廓的中心点y值肯定小于下轮廓的中心点y值
        if box2[0][1] > box1[0][1]:
            upBox = box1
            downBox = box2
        else:
            upBox = box2
            downBox = box1
        rectPointsUp = np.int0(cv.boxPoints(upBox))
        rectPointsDown = np.int0(cv.boxPoints(downBox))

        upLeftTop, upRightTop, upRightBottom, upLeftBottom = get_rect_points_by_clockwise(rectPointsUp)
        downLeftTop, downRightTop, downRightBottom, downLeftBottom = get_rect_points_by_clockwise(rectPointsDown)
        upWidth = distance(upLeftTop, upRightTop)
        upHeight = distance(upLeftTop, upLeftBottom)
        downWidth = distance(downLeftTop, downRightTop)
        downHeight = distance(downLeftTop, downLeftBottom)

        res["width"] = int((upWidth + downWidth) / 2 / MM2PIX)
        res["height"] = int((upHeight + downHeight) / 2 / MM2PIX)
        if upRightBottom[1] > downRightTop[1] + 25 or upLeftBottom[1] > downLeftTop[1] + 25:
            res["result"] = "fail"
            res["info"] = "识别异常,识别的线两块板边界重叠"
            return res
        elif upWidth < widthMin:
            res["result"] = "small"
            res["info"] = "上板识别错误,宽度识别值过小,可能尺寸填写过大造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                upWidth, upHeight, width, height, widthMin, widthMax)
            return res
        elif upWidth > widthMax:
            res["result"] = "big"
            res["info"] = "上板识别错误,宽度识别值过大,可能尺寸填写过小造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                upWidth, upHeight, width, height, widthMin, widthMax)
            return res
        elif upHeight < heightMin:
            res["result"] = "small"
            res["info"] = "上板识别错误,高度识别值过小,可能尺寸填写过大造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                upWidth, upHeight, width, height, widthMin, widthMax)
            return res
        elif upHeight > heightMax:
            res["result"] = "big"
            res["info"] = "上板识别错误,高度识别值过大,可能尺寸填写过小造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                upWidth, upHeight, width, height, widthMin, widthMax)
            return res
        elif downWidth < widthMin:
            res["result"] = "small"
            res["info"] = "下板识别错误,宽度识别值过小,可能尺寸填写过大造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                downWidth, downHeight, width, height, widthMin, widthMax)
            return res
        elif downWidth > widthMax:
            res["result"] = "big"
            res["info"] = "下板识别错误,宽度识别值过大,可能尺寸填写过小造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                downWidth, downHeight, width, height, widthMin, widthMax)
            return res
        elif downHeight < heightMin:
            res["result"] = "small"
            res["info"] = "下板识别错误,高度识别值过小,可能尺寸填写过大造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                downWidth, downHeight, width, height, widthMin, widthMax)
            return res
        elif downHeight > heightMax:
            res["result"] = "big"
            res["info"] = "下板识别错误,高度识别值过大,可能尺寸填写过小造成.识别结果: ({:.2f},{:.2f}),工单尺寸: ({:.2f},{:.2f}),阈值: ({:.2f},{:.2f})".format(
                downWidth, downHeight, width, height, widthMin, widthMax)
            return res
        else:
            res["result"] = "ok"
            res["upBox"] = [upLeftTop, upRightTop, upRightBottom, upLeftBottom]
            res["downBox"] = [downLeftTop, downRightTop, downRightBottom, downLeftBottom]
            res["info"] = ""
            res["calcSize"] = (res["width"], res["height"])
            res["calcErr"] = (abs(res["width"] - width / MM2PIX) + abs(res["height"] - height / MM2PIX))
            return res
    except Exception as e:
        res["result"] = "error"
        res["info"] = str(e)
        return res


def find_inner_board(innerBoard, width, height, calcThres, totalBoardColor):
    useImg = totalBoardColor.copy()
    boardArea = width * height
    res = {"result": "fail", "upBox": None, "downBox": None, "info": "", "calcSize": None, "calcErr": None}
    cnts = cv.findContours(innerBoard.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    cntsSorted = sorted(cnts, key=cv.contourArea, reverse=True)
    s1 = 0
    s2 = 0
    try:
        _, (w1, h1), _ = cv.minAreaRect(cntsSorted[0])
        s1 = w1 * h1
        _, (w2, h2), _ = cv.minAreaRect(cntsSorted[1])
        s2 = w2 * h2
    except Exception as e:
        pass

    # 1. 找轮廓面积,看看有几个轮廓.理论上来说,应该是有两个轮廓,如果只找到了一个.就要拆分成两个.
    if len(cnts) == 0:
        res["result"] = "fail"
        res["info"] = "can't find any box"
    elif s1 >= 1.7 * boardArea:
        innerBox = cv.minAreaRect(cntsSorted[0])
        rectPoints = np.int0(cv.boxPoints(innerBox))
        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_by_clockwise(rectPoints)
        topMid = np.add(leftTop, rightTop) / 2
        bottomMid = np.add(leftBottom, rightBottom) / 2
        # 定位椭圆圆心位置
        topX = topMid[0]
        topY = topMid[1] + height
        bottomX = bottomMid[0]
        bottomY = bottomMid[1] - height

        # 画椭圆,椭圆的圆心分别是上面的中点y方向-100,y方向+100
        ellipseR = int(height * 0.55)
        ellipseLong = int(width)
        upCenter = (int(topX), int(topY - ellipseR))
        downCenter = (int(bottomX), int(bottomY + ellipseR))
        ellipseImg = innerBoard.copy()
        cv.ellipse(ellipseImg, upCenter, (ellipseLong, ellipseR), 0, 0, 180, 0, 3)
        cv.ellipse(ellipseImg, downCenter, (ellipseLong, ellipseR), 0, 180, 360, 0, 3)
        image_show("Ellipse", ellipseImg, 1, True)

        # 再找一次轮廓,然后将轮廓画出来
        cnts = cv.findContours(ellipseImg, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cntsSorted = sorted(cnts, key=cv.contourArea, reverse=True)
        # 将轮廓画出来
        upRectBox = cv.minAreaRect(cntsSorted[0])
        downRectBox = cv.minAreaRect(cntsSorted[1])
        upRectPoints = np.int0(cv.boxPoints(upRectBox))
        downRectPoints = np.int0(cv.boxPoints(downRectBox))
        cv.drawContours(useImg, [upRectPoints, downRectPoints], -1, (0, 255, 0), 5)
        cv.imwrite("twoBoxes.bmp", useImg)
        image_show("TwoBoxes", useImg, 1, True)

        try:
            s1 = upRectBox[1][0] * upRectBox[1][1]
            s2 = downRectBox[1][0] * downRectBox[1][1]

            if 0.8 * boardArea < s1 < 1.2 * boardArea and 0.8 * boardArea < s2 < 1.2 * boardArea:
                res = check_box(cntsSorted, width, height, totalBoardColor, calcThres)
                return res
            res["result"] = "error"
            res["info"] = "拆分板后识别异常,一般是输入的高度尺寸过小引起的,请重新测量下板的高度."
            return res

        except Exception as e:
            res["result"] = "error"
            res["info"] = str(e)
            return res

    elif 0.7 * boardArea < s1 < 1.2 * boardArea and 0.7 * boardArea < s2 < 1.2 * boardArea:
        # 先将轮廓画出来
        upRectBox = cv.minAreaRect(cntsSorted[0])
        downRectBox = cv.minAreaRect(cntsSorted[1])
        upRectPoints = np.int0(cv.boxPoints(upRectBox))
        downRectPoints = np.int0(cv.boxPoints(downRectBox))
        cv.drawContours(useImg, [upRectPoints, downRectPoints], -1, (0, 255, 0), 5)
        image_show("TwoBoxes", useImg, 1, True)
        cv.imwrite("twoBoxes.bmp", useImg)
        res = check_box(cntsSorted, width, height, totalBoardColor, calcThres)
        return res
    else:
        res["result"] = "error"
        res["info"] = "找到的轮廓不满足要求"
        return res


def get_boxes(image, width, height, calcThres, tryTimes):
    rectWidth = max(int(width / 3), int(150 * MM2PIX))
    rectHeight = int(200 * MM2PIX)
    if image is None:
        return False, "图像异常,请重拍", None, None, None, None, None, None
    if len(image.shape) == 3:
        image = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
    image_show("Before", image, 1, True)
    imageGray, boxMaster = find_all_board(image, width, height, np.mean(image[:150, :1100]), 0.3)
    (cx, cy), (w, h), angle = boxMaster
    widthHalf = int(w / 2)
    heightHalf = int(h / 2)
    yStart = max(int(cy - heightHalf), 0)
    yEnd = min(int(cy + heightHalf), image.shape[0])
    xStart = max(int(cx - widthHalf), 0)
    xEnd = min(int(cx + widthHalf), image.shape[1])
    M = cv.getRotationMatrix2D((cx, cy), angle, 1)
    totalBoard = cv.warpAffine(imageGray, M, (image.shape[1], image.shape[0]))[yStart:yEnd, xStart:xEnd]
    image_show("TotalBoard", totalBoard, 1, True)
    totalBoardColor = cv.cvtColor(totalBoard, cv.COLOR_GRAY2BGR)
    blurred = cv.GaussianBlur(totalBoard, (0, 0), 10)
    scharrX = cv.Scharr(blurred, cv.CV_64F, 1, 0)
    scharrX = cv.convertScaleAbs(scharrX)
    scharrY = cv.Scharr(blurred, cv.CV_64F, 0, 1)
    scharrY = cv.convertScaleAbs(scharrY)
    scharrXY = cv.addWeighted(scharrX, 0.5, scharrY, 0.5, 0)
    image_show("ScharrXY ", scharrXY, 1, True)

    img32 = np.float32(scharrXY)
    square1 = cv.blur(np.multiply(img32, img32), (17, 17))
    square2 = np.multiply(cv.blur(img32, (17, 17)), cv.blur(img32, (17, 17)))
    squareDif = np.subtract(square1, square2)
    np.clip(squareDif, 0, 255, out=squareDif)
    contourImg = squareDif.astype("uint8")
    image_show("SquareDif", contourImg, 1, True)
    k1 = cv.getStructuringElement(cv.MORPH_RECT, (17, 17))
    for i in range(10):
        contourImg = cv.dilate(contourImg, k1)
    for i in range(10):
        contourImg = cv.erode(contourImg, k1)
    image_show("ScharrXYDilate", contourImg, 1, True)
    thresUsed = contourImg[rectHeight:int(contourImg.shape[0] / 2 - rectHeight), rectWidth:contourImg.shape[1] - rectWidth]
    print("FindInnerBoardThres T = {},thresOffset = {}".format(np.mean(thresUsed), 0))
    thresInit = 10
    offset = 0  # 识别的阈值补偿
    for calcTimes in range(20):
        _, contourImgNew = cv.threshold(contourImg, np.mean(thresUsed) + thresInit + offset, 255, cv.THRESH_BINARY)
        # 单独处理中缝,将中缝全部变成内板
        jointLineMask = np.zeros(image.shape, dtype=np.uint8)
        jointLineMask[:, 1106 - 10:1106 + 10] = 255
        jointLineMaskRoated = cv.warpAffine(jointLineMask, M, (image.shape[1], image.shape[0]))[yStart:yEnd, xStart:xEnd]
        contourImgNew = cv.subtract(contourImgNew, jointLineMaskRoated)
        image_show("InnerBoardThresDealed", contourImgNew, 1, True)

        # 5. 进行泛红填充,将所有的内板区域全部填充为150,内板全部处理成白色,内板之外全部处理为黑色
        mask = np.zeros((contourImgNew.shape[0] + 2, contourImgNew.shape[1] + 2), dtype=np.uint8)
        cv.floodFill(contourImgNew, mask, (contourImgNew.shape[1] // 2, contourImgNew.shape[0] // 4), 150)
        cv.floodFill(contourImgNew, mask, (contourImgNew.shape[1] // 2, contourImgNew.shape[0] * 3 // 4), 150)
        floodFill = np.zeros(contourImgNew.shape, dtype=np.uint8)
        floodFill[contourImgNew == 150] = 255
        image_show("FloodFill", floodFill, 1, True)

        # 6.进行去角以前的最后的腐蚀膨胀
        k2 = cv.getStructuringElement(cv.MORPH_RECT, (17, 17))
        for i in range(10):
            floodFill = cv.erode(floodFill, k2)
        for i in range(10):
            floodFill = cv.dilate(floodFill, k2)
        image_show("FloodFillErode", floodFill, 1, True)

        # 7.去掉四个角
        cv.rectangle(floodFill, (0, 0), (rectWidth, rectHeight), 0, -1)
        cv.rectangle(floodFill, (0, floodFill.shape[0]), (rectWidth, floodFill.shape[0] - rectHeight), 0, -1)
        cv.rectangle(floodFill, (floodFill.shape[1], 0), (floodFill.shape[1] - rectWidth, rectHeight), 0, -1)
        cv.rectangle(floodFill, (floodFill.shape[1], floodFill.shape[0]), (floodFill.shape[1] - rectWidth, floodFill.shape[0] - rectHeight), 0, -1)
        image_show("MoveAngle", floodFill, 0, True)

        # 8.去验证最后寻找到的内板轮廓
        res = find_inner_board(floodFill, width, height, calcThres, totalBoardColor)
        print("第{}次的识别结果,识别阈值:{},res:{}".format(calcTimes  , thresInit + offset, res))
        if res["result"] == "ok":
            return res["result"], res["info"], res["upBox"], res["downBox"], res["calcSize"], res["calcErr"], totalBoard
        elif res["result"] == "small":
            offset += 1
            continue
        elif res["result"] == "big":
            offset += -1
            continue
    else:
        print("没有计算出来")
        return None,None,None,None,None,None,None



def image_2_robot(imgXY, mtx):
    """图像坐标转换为机械手坐标"""
    imgX, imgY = imgXY
    robotX, robotY = np.dot((imgX, imgY, 1), np.transpose(mtx))
    return robotX, robotY

def get_linear_fit_k(pointsMat):
    # output 返回的四个值,前两个值是方向向量的x坐标,y坐标. 后面两个值是直线上的一个点.
    outPut = cv.fitLine(pointsMat, cv.DIST_L2, 0, 0.01, 0.01)
    if outPut[0] != 0:
        k = outPut[1] / outPut[0]
    else:
        k = None
    return k


def get_middle_of_two_points(p1, p2):
    """计算平面上两个点的中点"""
    middleX = (p1[0] + p2[0]) / 2
    middleY = (p1[1] + p2[1]) / 2
    return round(middleX, 2), round(middleY, 2)

# 已知一条直线的两个点,获取它的斜率k和b值
def get_line_k_and_b(p1, p2):
    if p1[0] == p2[0]:
        # 垂直于x轴,没有k值
        return None, p1[0]
    else:
        k = (p2[1] - p1[1]) / (p2[0] - p1[0])
        b = p1[1] - k * p1[0]
        return k, b
# 计算两条直线的交点,已知两条直线的k和b.
def get_two_lines_intersection(k1, b1, k2, b2):
    # 如果k相等,说明垂直无交点
    if k1 == k2:
        return None
    elif k1 is None and k2 is not None:
        return b1, k2 * b1 + b2
    elif k2 is None and k1 is not None:
        return b2, k1 * b2 + b1
    else:
        x = (b1 - b2) / (k1 - k2) * (-1)
        y = k1 * x + b1
        return x, y

# 根据四个点计算中缝.根据角平分线计算中缝.
def get_middle_cut_line(upLeftBottom, upRightBottom, downLeftTop, downRightTop):
    upK, upB = get_line_k_and_b(upLeftBottom, upRightBottom)
    downK, downB = get_line_k_and_b(downLeftTop, downRightTop)
    intersection = get_two_lines_intersection(upK, upB, downK, downB)
    # 如果本身就是平行线没有交点.这样中线的k就是upK 或者downK,都一样,如果确定b的值呢.
    if intersection is None:
        middleLineK = upK
        # 先找到中线上的一个必过点
        middleLineP = np.add(upLeftBottom, downLeftTop) / 2
        middleLineB = middleLineP[1] - middleLineK * middleLineP[0]
    else:
        middleLineK = np.tan((np.arctan(upK) + np.arctan(downK)) / 2)  # 角平分线公式
        middleLineB = intersection[1] - middleLineK * intersection[0]

    return middleLineK, middleLineB


def get_lines_with_boxes(img, upBox, downBox, mtx, topPix, leftPix, bottomPix, rightPix, width, height, totalBoard):
    if len(img.shape) == 3:
        img = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
    imgColor = cv.cvtColor(img, cv.COLOR_GRAY2BGR)
    imageGray, boxMaster = find_all_board(img, width, height, np.mean(img[:150, :1100]), 0.3)
    (bigX, bigY), (w, h), angle = boxMaster

    upLeftTop, upRightTop, upRightBottom, upLeftBottom = upBox
    downLeftTop, downRightTop, downRightBottom, downLeftBottom = downBox

    leftCutLineK = get_linear_fit_k(np.array([upLeftTop, upLeftBottom, downLeftTop, downLeftBottom]))
    if leftCutLineK is None:
        leftCutLineB = min(upLeftTop[0], upLeftBottom[0], downLeftTop[0], downLeftBottom[0]) - leftPix
    else:
        leftFourB = [y - (x - leftPix) * leftCutLineK for (x, y) in [upLeftTop, upLeftBottom, downLeftTop, downLeftBottom]]
        if leftCutLineK > 0:
            leftCutLineB = max(leftFourB)
        else:
            leftCutLineB = min(leftFourB)

    rightCutLineK = get_linear_fit_k(np.array([upRightTop, upRightBottom, downRightTop, downRightBottom]))
    if rightCutLineK is None:
        rightCutLineB = max(upRightTop[0], upRightBottom[0], downRightTop[0], downRightBottom[0]) + rightPix
    else:
        rightFourB = [y - (x + rightPix) * rightCutLineK for (x, y) in [upRightTop, upRightBottom, downRightTop, downRightBottom]]
        if rightCutLineK > 0:
            rightCutLineB = min(rightFourB)
        else:
            rightCutLineB = max(rightFourB)

    topCutLineK = get_line_k_and_b(upLeftTop, upRightTop)[0]
    if topCutLineK is None:
        return False, None, None, None, None
    else:
        topCutLineB = upLeftTop[1] - topPix - topCutLineK * upLeftTop[0]

    bottomCutLineK = get_line_k_and_b(downLeftBottom, downRightBottom)[0]
    if bottomCutLineK is None:
        return False, None, None, None, None
    else:
        bottomCutLineB = downLeftBottom[1] + bottomPix - bottomCutLineK * downLeftBottom[0]

    # 求中缝.我用自己的方法求中缝.传进去四个点.然后求这个四个点的角平分线.
    middleCutLineK, middleCutLineB = get_middle_cut_line(upLeftBottom, upRightBottom, downLeftTop, downRightTop)

    # 下面是求1,3刀不切,或者2,4刀不切的时候的流程.去掉板宽的1/4,这样做的目的是将两边容易变形的铜箔过滤掉
    boardNoCut13 = totalBoard.copy()
    boardNoCut13[:, :int(width / 3)] = 0
    boardNoCut13[:, int(w - width * 1 / 3):] = 0
    image_show("Board13", boardNoCut13, 1, True)
    _, box13 = find_all_board(boardNoCut13, width / 3, height, np.mean(img[:150, :1100]), 0.01)
    box13Points = np.int0(cv.boxPoints(box13))
    leftTop13, rightTop13, rightBottom13, leftBottom13 = get_rect_points_by_clockwise(box13Points)

    boardNoCut24 = totalBoard.copy()
    boardNoCut24[:int(height / 3), :] = 0
    boardNoCut24[int(h - height / 3):, :] = 0
    image_show("Board24", boardNoCut24, 1, True)
    _, box24 = find_all_board(boardNoCut24, width, height * 2 / 3, np.mean(img[:150, :1100]), 0.01)
    box24Points = np.int0(cv.boxPoints(box24))
    leftTop24, rightTop24, rightBottom24, leftBottom24 = get_rect_points_by_clockwise(box24Points)

    # 求当边不切的时候,重新算斜率和b值
    if topPix == 0:
        topCutLineK, topCutLineB = get_line_k_and_b(leftTop13, rightTop13)
    if bottomPix == 0:
        bottomCutLineK, bottomCutLineB = get_line_k_and_b(leftBottom13, rightBottom13)
    if leftPix == 0:
        leftCutLineK, leftCutLineB = get_line_k_and_b(leftTop24, leftBottom24)
    if rightPix == 0:
        rightCutLineK, rightCutLineB = get_line_k_and_b(rightTop24, rightBottom24)

    # 求所有线的交点
    leftTopIntersection = get_two_lines_intersection(leftCutLineK, leftCutLineB, topCutLineK, topCutLineB)
    rightTopIntersection = get_two_lines_intersection(rightCutLineK, rightCutLineB, topCutLineK, topCutLineB)
    leftMiddleIntersection = get_two_lines_intersection(leftCutLineK, leftCutLineB, middleCutLineK, middleCutLineB)
    rightMiddleIntersection = get_two_lines_intersection(rightCutLineK, rightCutLineB, middleCutLineK, middleCutLineB)
    leftBottomIntersection = get_two_lines_intersection(leftCutLineK, leftCutLineB, bottomCutLineK, bottomCutLineB)
    rightBottomIntersection = get_two_lines_intersection(rightCutLineK, rightCutLineB, bottomCutLineK, bottomCutLineB)

    if leftTopIntersection is None or rightTopIntersection is None or leftBottomIntersection is None or rightBottomIntersection is None:
        return False, None, None, None, None

    # 交点求出来之后,就是转换为大图上面,然后延长线的问题.
    extend = int(8 * MM2PIX)  # 线延长8个mm

    topCutLeftPoint = [-extend, topCutLineK * (-extend) + topCutLineB]
    topCutRightPoint = [w + extend, topCutLineK * (w + extend) + topCutLineB]
    bottomCutLeftPoint = [-extend, bottomCutLineK * (leftBottomIntersection[0] - extend) + bottomCutLineB]
    bottomCutRightPoint = [w + extend, bottomCutLineK * (w + extend) + bottomCutLineB]

    if leftCutLineK is None:
        leftCutUpPoint = [leftCutLineB, leftTopIntersection[1] - extend]
        leftCutDownPoint = [leftCutLineB, leftBottomIntersection[1] + extend]
    elif leftCutLineK != 0:
        leftCutUpPoint = [(leftTopIntersection[1] - extend - leftCutLineB) / leftCutLineK, leftTopIntersection[1] - extend]
        leftCutDownPoint = [(leftBottomIntersection[1] + extend - leftCutLineB) / leftCutLineK, leftBottomIntersection[1] + extend]
    else:
        return False, None, None, None, None

    if rightCutLineK is None:
        rightCutUpPoint = [rightCutLineB, rightTopIntersection[1] - extend]
        rightCutDownPoint = [rightCutLineB, rightBottomIntersection[1] + extend]
    elif rightCutLineK != 0:
        rightCutUpPoint = [(rightTopIntersection[1] - extend - rightCutLineB) / rightCutLineK, rightTopIntersection[1] - extend]
        rightCutDownPoint = [(rightBottomIntersection[1] + extend - rightCutLineB) / rightCutLineK, rightBottomIntersection[1] + extend]

    middleCutLeftPoint = leftMiddleIntersection
    middleCutRightPoint = rightMiddleIntersection

    smallX, smallY = w // 2 - 1, h // 2 - 1  # 先求截取的板的中心点,然后求和大图的中心点偏移值.将所有的点都按照这个偏移值加起来就可以转化为大图上去.
    M = cv.getRotationMatrix2D((smallX, smallY), -angle, 1)
    # 两个中心点的差值
    centerOffset = [bigX - smallX, bigY - smallY]
    topCutLeftPointBig = np.add(np.dot(M, [topCutLeftPoint[0], topCutLeftPoint[1], 1]), centerOffset)
    topCutRightPointBig = np.add(np.dot(M, [topCutRightPoint[0], topCutRightPoint[1], 1]), centerOffset)
    bottomCutLeftPointBig = np.add(np.dot(M, [bottomCutLeftPoint[0], bottomCutLeftPoint[1], 1]), centerOffset)
    bottomCutRightPointBig = np.add(np.dot(M, [bottomCutRightPoint[0], bottomCutRightPoint[1], 1]), centerOffset)
    middleCutLeftPointBig = np.add(np.dot(M, [middleCutLeftPoint[0], middleCutLeftPoint[1], 1]), centerOffset)
    middleCutRightPointBig = np.add(np.dot(M, [middleCutRightPoint[0], middleCutRightPoint[1], 1]), centerOffset)
    leftCutUpPointBig = np.add(np.dot(M, [leftCutUpPoint[0], leftCutUpPoint[1], 1]), centerOffset)
    leftCutDownPointBig = np.add(np.dot(M, [leftCutDownPoint[0], leftCutDownPoint[1], 1]), centerOffset)
    rightCutUpPointBig = np.add(np.dot(M, [rightCutUpPoint[0], rightCutUpPoint[1], 1]), centerOffset)
    rightCutDownPointBig = np.add(np.dot(M, [rightCutDownPoint[0], rightCutDownPoint[1], 1]), centerOffset)

    # 重新将找到的几个点定位到大图上去,然后将这个点画出来
    twoBoxesPoints = [upLeftTop, upRightTop, upLeftBottom, upRightBottom, downLeftTop, downRightTop, downLeftBottom, downRightBottom]
    twoBoxesPointsBig = [np.add(np.dot(M, [p[0], p[1], 1]), centerOffset) for p in twoBoxesPoints]

    for pInBig in twoBoxesPointsBig:
        cv.circle(imgColor, (int(pInBig[0]), int(pInBig[1])), int(MM2PIX * 2), (0, 0, 255), 5)

    # 画出上,下,左,右,中 五条切线
    cutLineColor = (0, 255, 0)
    cutLineThickness = 6
    textColor = (255, 0, 0)
    textSize = 12
    topCutMiddle, leftCutMiddle, rightCutMiddle, bottomCutMiddle = [get_middle_of_two_points(line[0], line[1]) for line in
                                                                    [(topCutLeftPointBig, topCutRightPointBig),
                                                                     (leftCutUpPointBig, leftCutDownPointBig),
                                                                     (rightCutUpPointBig, rightCutDownPointBig),
                                                                     (bottomCutLeftPointBig, bottomCutRightPointBig)]]
    cv.line(imgColor, tuple(np.int0(topCutLeftPointBig)), tuple(np.int0(topCutRightPointBig)), cutLineColor, cutLineThickness)
    cv.putText(imgColor, "1", (int(topCutMiddle[0]), int(topCutMiddle[1]) - 150), cv.FONT_HERSHEY_COMPLEX, 4, textColor, textSize)
    cv.line(imgColor, tuple(np.int0(leftCutUpPointBig)), tuple(np.int0(leftCutDownPointBig)), cutLineColor, cutLineThickness)
    cv.putText(imgColor, "2", (int(leftCutMiddle[0]) - 150, int(leftCutMiddle[1])), cv.FONT_HERSHEY_COMPLEX, 4, textColor, textSize)
    cv.line(imgColor, tuple(np.int0(bottomCutLeftPointBig)), tuple(np.int0(bottomCutRightPointBig)), cutLineColor, cutLineThickness)
    cv.putText(imgColor, "3", (int(bottomCutMiddle[0]), int(bottomCutMiddle[1]) + 150), cv.FONT_HERSHEY_COMPLEX, 4, textColor, textSize)
    cv.line(imgColor, tuple(np.int0(rightCutUpPointBig)), tuple(np.int0(rightCutDownPointBig)), cutLineColor, cutLineThickness)
    cv.putText(imgColor, "4", (int(rightCutMiddle[0]) + 150, int(rightCutMiddle[1])), cv.FONT_HERSHEY_COMPLEX, 4, textColor, textSize)
    cv.line(imgColor, tuple(np.int0(middleCutLeftPointBig)), tuple(np.int0(middleCutRightPointBig)), cutLineColor, cutLineThickness)
    image_show("ResultImage", imgColor, 0, True)

    # 将所有的点全部转换为机械手坐标,利用标定矩阵
    topCutLeftPointRob = image_2_robot(topCutLeftPointBig, mtx)
    topCutRightPointRob = image_2_robot(topCutRightPointBig, mtx)
    leftCutUpPointRob = image_2_robot(leftCutUpPointBig, mtx)
    leftCutDownPointRob = image_2_robot(leftCutDownPointBig, mtx)
    bottomCutLeftPointRob = image_2_robot(bottomCutLeftPointBig, mtx)
    bottomCutRightPointRob = image_2_robot(bottomCutRightPointBig, mtx)
    rightCutUpPointRob = image_2_robot(rightCutUpPointBig, mtx)
    rightCutDownPointRob = image_2_robot(rightCutDownPointBig, mtx)
    middleCutLeftPointRob = image_2_robot(middleCutLeftPointBig, mtx)
    middleCutRightPointRob = image_2_robot(middleCutRightPointBig, mtx)

    cutDis = 0  # 切割面积
    if topPix != 0:
        cutDis += distance(topCutLeftPointRob, topCutRightPointRob)
    if leftPix != 0:
        cutDis += distance(leftCutUpPointRob, leftCutDownPointRob)
    if bottomPix != 0:
        cutDis += distance(bottomCutLeftPointRob, bottomCutRightPointRob)
    if rightPix != 0:
        cutDis += distance(rightCutUpPointRob, rightCutDownPointRob)

    resultImg = cv.resize(imgColor, (imgColor.shape[1] // 2, imgColor.shape[0] // 2), cv.INTER_AREA)

    # 计算板长,第一块.和总板长
    topToMiddle, topToBottom = get_board_length([topCutLeftPointRob, topCutRightPointRob], [middleCutLeftPointRob, middleCutRightPointRob],
                                                [bottomCutLeftPointRob, bottomCutRightPointRob])
    # 计算角度,注意点的方向必须一致,不然计算的角度可能是反的.全部计算从中缝到第一刀或者第三刀的角度.如果是正值,表示要逆时针调整
    topMidAngle = get_two_lines_angle(middleCutRightPointRob, middleCutLeftPointRob, topCutRightPointRob, topCutLeftPointRob)
    bottomMidAngle = get_two_lines_angle(middleCutRightPointRob, middleCutLeftPointRob, bottomCutRightPointRob, bottomCutLeftPointRob)

    return [topCutRightPointRob, topCutLeftPointRob, leftCutUpPointRob, leftCutDownPointRob,
            bottomCutLeftPointRob, bottomCutRightPointRob, rightCutDownPointRob, rightCutUpPointRob,
            middleCutLeftPointRob, middleCutRightPointRob], resultImg, cutDis, [topToMiddle, topToBottom], [topMidAngle, bottomMidAngle]

def get_board_length(topLine, midLine, bottomLine):
    """根据三条线,以及灯管间距获取板的高度,以及总高度"""
    # 将x和y互换,换成我们习惯的坐标,以裁切机为原点.
    lineNew = []
    for line in [topLine, midLine, bottomLine]:
        lineTmp = [[line[0][1], line[0][0]], [line[1][1], line[1][0]]]
        lineNew.append(lineTmp)
    topLine, midLine, bottomLine = lineNew
    midLineMidP = get_middle_of_two_points(midLine[0], midLine[1])
    # 计算板长,用垂直于中线中点的垂线与topLine的交点,以及和bottomLine的交点来求
    topK, topB = get_line_k_and_b(topLine[0], topLine[1])
    midK, midB = get_line_k_and_b(midLine[0], midLine[1])
    bottomK, bottomB = get_line_k_and_b(bottomLine[0], bottomLine[1])

    if midK == 0:
        midVerTopIntersection = [midLineMidP[0], topK * midLineMidP[0] + topB]
        midVerBottomIntersection = [midLineMidP[0], bottomK * midLineMidP[0] + bottomB]
    else:
        midVerK = -1 / midK
        midVerB = midLineMidP[1] - midVerK * midLineMidP[0]
        midVerTopIntersection = get_two_lines_intersection(midVerK, midVerB, topK, topB)
        midVerBottomIntersection = get_two_lines_intersection(midVerK, midVerB, bottomK, bottomB)
    topToMiddle = distance(midLineMidP, midVerTopIntersection)
    topToBottom = distance(midLineMidP, midVerBottomIntersection) + topToMiddle

    return round(topToMiddle, 2), round(topToBottom, 2)

def get_two_lines_angle(p1, p2, p3, p4):
    line1 = Line(p1[0], p1[1], p2[0], p2[1])
    line2 = Line(p3[0], p3[1], p4[0], p4[1])
    v1 = line1.vector()
    v2 = line2.vector()
    length1 = line1.length()
    length2 = line2.length()

    # 先求向量叉乘,确定方向. 如果是正值,表示1到2需要逆时针,调整.跟我们规定的逆时针调整为正刚好吻合
    derection = v1[0] * v2[1] - v1[1] * v2[0]
    # 获取内积
    innerMul = np.dot(v1, v2)
    modelMul = length1 * length2
    # 向量夹角公式
    cosX = innerMul / modelMul
    # 防止计算误差,出现cosx>1的情况,例如1.000003等的情况
    cosX = 1 if cosX > 1 else cosX
    includeAngle = np.arccos(cosX) * (180 / np.pi)
    includeAngle = includeAngle if derection > 0 else -includeAngle
    return round(includeAngle, 2)

class Line(object):
    def __init__(self, x1, y1, x2, y2):
        self.x1 = x1
        self.y1 = y1
        self.x2 = x2
        self.y2 = y2

    def vector(self):
        c = (self.x1 - self.x2, self.y1 - self.y2)
        return c

    def length(self):
        length = np.sqrt(np.square(self.vector()[0]) + np.square(self.vector()[1]))
        return length


if __name__ == '__main__':
    tryBegin = 2
    tryEnd = 3
    filePath = r"D:\chongda_raw\can't_find_middle\188--2020-09-21_16-45-46--w_1989--h_1745.bmp"
    MM2PIX = 3.2038
    mtx = [[0, 1, 0], [1, 0, 0]]
    nowTime = time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime())
    keepDir = filePath
    if os.path.isfile(filePath):
        keepDir, _ = os.path.split(filePath)

    # 保存成功结果的实际路径
    # 保存失败图片的实际路径
    keepResPath = os.path.join(keepDir, nowTime.split("_")[0] + "_res")
    keepFailPath = os.path.join(keepDir, nowTime.split("_")[0] + "_fail")

    # 如果两个目录都不存在,这创建
    if not os.path.exists(keepResPath):
        os.mkdir(keepResPath)
    if not os.path.exists(keepFailPath):
        os.mkdir(keepFailPath)

    calcThres = 12
    fileNames = []
    remainEdge = [1, 1, 1, 1]
    noCut = False if any(remainEdge) else True
    topPix, leftPix, bottomPix, rightPix = [int(x * MM2PIX) for x in remainEdge]
    if os.path.isfile(filePath):
        filePath, fileName = os.path.split(filePath)
        fileNames.append(fileName)
    else:
        for root, dirs, files in os.walk(filePath):
            if root != filePath:  # 过滤掉不是要处理的图像的目录
                continue
            for file in files:
                fileNames.append(file)

    for index, fileName in enumerate(fileNames):
        filePathReal = os.path.join(filePath, fileName)
        width, height = get_width_and_height(fileName)
        if width is None or height is None:
            print("*" * 10 + " 第{} 张图 ({}) 图像名称错误,已经跳过".format(index + 1, fileName) + "*" * 10)
            continue
        print("*" * 10 + " 第{} 张图 ({})".format(index + 1, fileName) + "*" * 10)
        img = cv.imread(filePathReal, cv.IMREAD_GRAYSCALE)
        resOkList = []  # 识别成功的列表
        resSizeDifList = []  # 识别尺寸出问题的列表
        totalBoard = None
        for tryTimes in range(tryBegin, tryEnd):
            # res, info, upBox, downBox, boardSize, boardError, totalBoard = get_boxes_(img, width, height, calcThres, tryTimes, calcArgs)
            res, info, upBox, downBox, boardSize, boardError, totalBoard = get_boxes(img, width, height, calcThres, tryTimes)
            if res == "ok":
                resOkList.append((boardError, upBox, downBox, info, boardSize))
                break
            elif res == "sizeDif":
                resSizeDifList.append((boardError, upBox, downBox, info, boardSize))

        print("识别成功的结果 = {},个数 = {}".format(resOkList, len(resOkList)))
        print("识别尺寸出问题的结果 = {},个数 = {}".format(resSizeDifList, len(resSizeDifList)))
        if len(resOkList) == 0:
            # 识别失败,将图片保存到失败的目录下面
            cv.imwrite(os.path.join(keepFailPath, fileName), img)
            continue
        resSorted = sorted(resOkList, key=lambda x: x[0])
        upBox, downBox, info, boardSize = resSorted[0][1:]

        assert upBox is not None
        assert downBox is not None
        resPath = os.path.join(keepResPath, fileName)
        cutLines, imgRes, cutDis, boardLength, angleList = get_lines_with_boxes(img, upBox, downBox, mtx, topPix, leftPix, bottomPix, rightPix, width,
                                                                                height, totalBoard)
        cv.imwrite(resPath, imgRes)
